#include <string>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Range.h>

#include "Driver.h"
#include "WallFollower.h"
#include "utilities/MsgParser.h"
#include "utilities/Constants.h"

using namespace std;

#define MSG_PARAMS_NUM 3 // number of parameters in a message for Rover Module
#define MSG_PARAMS_DELIMITER " " // delimiter of parameters in a message for Rover Module

// object in charge of publishing commands to wheels- will be allocated in main
Driver* driver;
WallFollower* follower;

// ROS management objects
ros::Rate* loop_rate;
ros::Publisher sensors_publisher;
std_msgs::String message_to_publish;

/**
 * Read published commands and respond accordingly
 */
void commandsHandler(const std_msgs::String::ConstPtr& msg) {

  	ROS_ERROR("/Rover_Module/Driver received message.");

	// parse published message

	// switch case:

	// case A:
		// call method

  			// allocate a wall-following manager object
  			follower = new WallFollower(driver, WALL_FOLLOW_SPEED, 0.4);

  			// start wall following
  			follower->follow();

			// ->sample global values in order to get published data

	// case B:...
			// allocate task-B manager object and start...
}

/**
 * Listens to "driver" topic until shuts down.
 * Parses published messages and uses the call-back function
 * to perform the commands
 */
int main(int argc, char **argv) {

	// initialize system
	ros::init(argc, argv, "RoverMngrNode");
	ros::NodeHandle node_handler;

	loop_rate = new ros::Rate(1);

	// allocating the rover controller
	driver = new Driver();

	// receive commands from somebody (now - terminal)
	ros::Subscriber sub_command_receive = node_handler.subscribe("/Rover_Module/Driver",1000,commandsHandler);

	// keep listening to subscribers until ros::shutdown() has been called and is finished
	while (ros::ok()) {
		ros::spinOnce();
		//main_loop_rate.sleep();
	}
	// done listening

	delete driver;
	delete follower;
	return 0;
}
